
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<math.h>
#include"vmatrix.h"

#include"glheader.h"

#ifndef M_PI
#define M_PI       3.14159265358979323846264338
#endif

struct mstack{
  float m[16];
  VMStack * prev;
  VMStack * next;
  mstack (): prev(NULL),next(NULL) {}
  ~mstack (){
    if(next!=NULL)
      delete next;
  }
  void SetNext (mstack * n){
    n->prev=this;
    next = n;
  }
};

VMatrix::VMatrix (): mode(VM_MODELVIEW), tex(false)
{
  base = new mstack();
  top = base;
}

VMatrix::~VMatrix ()
{
  delete base;
}

void VMatrix::SetMode (int md)
{
  mode = md;
}

void VMatrix::Push ()
{
  if(mode==VM_PROJECTION)
    return;
  mstack * temp;
  //só aloca se for preciso
  if(top->next==NULL){
    temp = new mstack();
    top->SetNext(temp);
  }
  else
    temp = top->next;
  //copia o topo anterior para o novo topo
  for(int i=0;i<16;i++)
    temp->m[i] = top->m[i];
  top = temp;
}

//se a matriz corrente for a base não faz nada
//nao desaloca o topo
void VMatrix::Pop ()
{
  if(mode==VM_PROJECTION)
    return;
  if(top != base)
    top = top->prev;
}

void VMatrix::LoadIdentity ()
{
  float * m = (mode==VM_PROJECTION)?proj:top->m;
  m[0] = 1; m[4] = 0; m[8] = 0;  m[12] = 0;
  m[1] = 0; m[5] = 1; m[9] = 0;  m[13] = 0;
  m[2] = 0; m[6] = 0; m[10] = 1; m[14] = 0;
  m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;
}

void VMatrix::LoadMatrix (const float* mat)
{
  float * m = (mode==VM_PROJECTION)?proj:top->m;
  for(int i=0;i<16;i++)
    m[i] = mat[i];
}

static void multmatrix_aux (const float *c, const float *m, float* dest)
{
  float res[16];
  res[0] = c[0]*m[0] + c[4]*m[1] + c[8]*m[2]  + c[12]*m[3];
  res[1] = c[1]*m[0] + c[5]*m[1] + c[9]*m[2]  + c[13]*m[3];
  res[2] = c[2]*m[0] + c[6]*m[1] + c[10]*m[2] + c[14]*m[3];
  res[3] = c[3]*m[0] + c[7]*m[1] + c[11]*m[2] + c[15]*m[3];
  //------------------------------------------------------
  res[4] = c[0]*m[4] + c[4]*m[5] + c[8]*m[6]  + c[12]*m[7];
  res[5] = c[1]*m[4] + c[5]*m[5] + c[9]*m[6]  + c[13]*m[7];
  res[6] = c[2]*m[4] + c[6]*m[5] + c[10]*m[6] + c[14]*m[7];
  res[7] = c[3]*m[4] + c[7]*m[5] + c[11]*m[6] + c[15]*m[7];
  //------------------------------------------------------
  res[8]  = c[0]*m[8] + c[4]*m[9] + c[8]*m[10]  + c[12]*m[11];
  res[9]  = c[1]*m[8] + c[5]*m[9] + c[9]*m[10]  + c[13]*m[11];
  res[10] = c[2]*m[8] + c[6]*m[9] + c[10]*m[10] + c[14]*m[11];
  res[11] = c[3]*m[8] + c[7]*m[9] + c[11]*m[10] + c[15]*m[11];
  //------------------------------------------------------
  res[12]  = c[0]*m[12] + c[4]*m[13] + c[8]*m[14]   + c[12]*m[15];
  res[13]  = c[1]*m[12] + c[5]*m[13] + c[9]*m[14]   + c[13]*m[15];
  res[14]  = c[2]*m[12] + c[6]*m[13] + c[10]*m[14]  + c[14]*m[15];
  res[15]  = c[3]*m[12] + c[7]*m[13] + c[11]*m[14]  + c[15]*m[15];
  //------------------------------------------------------
  for(int i=0;i<16;i++)
    dest[i] = res[i];

}

void VMatrix::MultMatrix (const float* m)
{
  float * c = (mode==VM_PROJECTION)?proj:top->m;
  multmatrix_aux(c,m,c);
}

void VMatrix::Translate (float x, float y, float z)
{
  float * mat = (mode==VM_PROJECTION)?proj:top->m;
  int a=0,b=4,c=8,d=12;
  for(int i=0;i<4;i++)
    mat[d+i] = mat[a+i]*x + mat[b+i]*y + mat[c+i]*z + mat[d+i];
}

void VMatrix::Scale (float x, float y, float z)
{
  float * c = (mode==VM_PROJECTION)?proj:top->m;
  c[0]*=x; c[1]*=x; c[2]*=x; c[3]*=x; 
  c[4]*=y; c[5]*=y; c[6]*=y; c[7]*=y;
  c[8]*=z; c[9]*=z; c[10]*=z; c[11]*=z;
}

void VMatrix::Rotate (float a, float x, float y, float z)
{
  float m[16];
  float norm = (float) sqrt(x*x + y*y + z*z);
  /* Converte graus para radianos. */
  float angle = (a* (float) M_PI)/180;
  if ( norm >= 1e-9 ) {
    float cos_a, sin_a;
    cos_a = (float) cos(angle);
    sin_a = (float) sin(angle);
    x /= norm; y /= norm; z /= norm;
    /* Linha 1 */
    m[0]  = cos_a + (1 - cos_a)*x*x;    
    m[4]  = x*y*(1 - cos_a) - z*sin_a;
    m[8]  = z*x*(1 - cos_a) + y*sin_a;
    m[12] =  0;
    /* Linha 2 */
    m[1]  = x*y*(1 - cos_a) + z*sin_a;
    m[5]  = cos_a + (1 - cos_a)*y*y;
    m[9]  = y*z*(1 - cos_a) - x*sin_a;
    m[13] = 0;
    /* Linha 3 */
    m[2]  = x*z*(1 - cos_a) - y*sin_a;
    m[6]  = y*z*(1 - cos_a) + x*sin_a;
    m[10] = cos_a + (1 - cos_a)*z*z;
    m[14] = 0;
    /* Linha 4 */
    m[3]  = 0;
    m[7]  = 0;
    m[11] = 0;
    m[15] = 1;
  }
  else {
    /* Matriz identidade. */
    m[0] = 1; m[4] = 0; m[8] = 0;  m[12] = 0;
    m[1] = 0; m[5] = 1; m[9] = 0;  m[13] = 0;
    m[2] = 0; m[6] = 0; m[10] = 1; m[14] = 0;
    m[3] = 0; m[7] = 0; m[11] = 0; m[15] = 1;    
  }
  MultMatrix(m);
}

static void cross_prod (float * a,float * b,float * c)
{
    c[0] = a[1]*b[2] - a[2]*b[1];
    c[1] = a[2]*b[0] - a[0]*b[2];
    c[2] = a[0]*b[1] - a[1]*b[0];
}

void VMatrix::LookAt (float ex, float ey, float ez, 
                      float cx, float cy, float cz, 
                      float ux, float uy, float uz)
{
  float f[3]  = {cx-ex,cy-ey,cz-ez};
  float fnorm = (float) sqrt(f[0]*f[0] + f[1]*f[1] + f[2]*f[2]);
  float up[3] = {ux,uy,uz};
  float upnorm = (float) sqrt(up[0]*up[0] + up[1]*up[1] + up[2]*up[2]);
  f[0]/=fnorm; up[0]/=upnorm;
  f[1]/=fnorm; up[1]/=upnorm;
  f[2]/=fnorm; up[2]/=upnorm;
  float s[3], u[3];
  cross_prod(f,up,s);
  float snorm = (float) sqrt(s[0]*s[0] + s[1]*s[1] + s[2]*s[2]);
  s[0]/=snorm;
  s[1]/=snorm;
  s[2]/=snorm;
  cross_prod(s,f,u);
  float m[16];
  m[0]= s[0];  m[4]= s[1];  m[8]= s[2];   m[12]= 0;
  m[1]= u[0];  m[5]= u[1];  m[9]= u[2];   m[13]= 0;
  m[2]= -f[0]; m[6]= -f[1]; m[10]= -f[2]; m[14]= 0;
  m[3]= 0;     m[7]= 0;     m[11]= 0;     m[15]= 1;
  MultMatrix(m);
  Translate(-ex, -ey, -ez);
}

void VMatrix::Frustum (float xmin, float xmax,  float ymin,
                       float ymax, float znear, float zfar)
{
  float a = (xmax+xmin)/(xmax-xmin);
  float b = (ymax+ymin)/(ymax-ymin);
  float c = -(zfar+znear)/(zfar-znear);
  float d = -2*zfar*znear/(zfar-znear);
  float m[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
  m[0] = 2*znear/(xmax-xmin);
  m[5] = 2*znear/(ymax-ymin);
  m[8] = a;
  m[9] = b;
  m[10] = c;
  m[11] = -1;
  m[14] = d;
  MultMatrix(m);
}

void VMatrix::Perspective (float fovy, float aspect, float znear, float zfar)
{
  float y = tan( fovy / 360 * M_PI ) * znear;    
  float x = y*aspect;
  Frustum(-x,x,-y,y,znear,zfar);
}

void VMatrix::GetMatrix (float* m) const
{
  const float * c = (mode==VM_PROJECTION)?proj:top->m;
  for(int i=0;i<16;i++)
    m[i] = c[i];
}

static void inverse (float * m_v)
{
  float Adj[16];    // Cofactors and adjoint matrix
  float Det2x2[12]; // 2x2 sub matrix determinants

  float m0  = m_v[0];
  float m1  = m_v[1];
  float m2  = m_v[2];
  float m3  = m_v[3];
  float m4  = m_v[4];
  float m5  = m_v[5];
  float m6  = m_v[6];
  float m7  = m_v[7];
  float m8  = m_v[8];
  float m9  = m_v[9];
  float m10 = m_v[10];
  float m11 = m_v[11];
  float m12 = m_v[12];
  float m13 = m_v[13];
  float m14 = m_v[14];
  float m15 = m_v[15];

  // Auxiliary determinants of 2x2 sub-matrices

  Det2x2[0]  = m10 * m15 - m11 * m14;
  Det2x2[1]  = m9  * m15 - m11 * m13;
  Det2x2[2]  = m9  * m14 - m10 * m13;
  Det2x2[3]  = m8  * m15 - m11 * m12;
  Det2x2[4]  = m8  * m14 - m10 * m12;
  Det2x2[5]  = m8  * m13 - m9  * m12;

  // First 4 cofactors

  Adj[0]  =   m5 * Det2x2[0] - m6 * Det2x2[1]  + m7 * Det2x2[2];
  Adj[1]  = -(m4 * Det2x2[0] - m6 * Det2x2[3]  + m7 * Det2x2[4]);
  Adj[2]  =   m4 * Det2x2[1] - m5 * Det2x2[3]  + m7 * Det2x2[5];
  Adj[3]  = -(m4 * Det2x2[2] - m5 * Det2x2[4]  + m6 * Det2x2[5]);

  // Calculation and verification of the 4x4 matrix determinant

  float Det4x4 = m0 * Adj[0]
               + m1 * Adj[1]
               + m2 * Adj[2]
               + m3 * Adj[3];

  if (Det4x4 == 0.0f)     // The matrix is not invertible! Return a matrix filled with zeros
  {
    m_v[0] = 0.0f; m_v[4] = 0.0f; m_v[8]  = 0.0f; m_v[12] = 0.0f;
    m_v[1] = 0.0f; m_v[5] = 0.0f; m_v[9]  = 0.0f; m_v[13] = 0.0f;
    m_v[2] = 0.0f; m_v[6] = 0.0f; m_v[10] = 0.0f; m_v[14] = 0.0f;
    m_v[3] = 0.0f; m_v[7] = 0.0f; m_v[11] = 0.0f; m_v[15] = 0.0f;
    //return false;
  }

  // Other auxiliary determinants of 2x2 sub-matrices

  Det2x2[6]  = m2  * m7  - m3  * m6;
  Det2x2[7]  = m1  * m7  - m3  * m5;
  Det2x2[8]  = m1  * m6  - m2  * m5;
  Det2x2[9]  = m0  * m7  - m3  * m4;
  Det2x2[10] = m0  * m6  - m2  * m4;
  Det2x2[11] = m0  * m5  - m1  * m4;

  // Other 12 cofactors

  Adj[4]  = -(m1 * Det2x2[0] - m2 * Det2x2[1]  + m3 * Det2x2[2]);
  Adj[5]  =   m0 * Det2x2[0] - m2 * Det2x2[3]  + m3 * Det2x2[4];
  Adj[6]  = -(m0 * Det2x2[1] - m1 * Det2x2[3]  + m3 * Det2x2[5]);
  Adj[7]  =   m0 * Det2x2[2] - m1 * Det2x2[4]  + m2 * Det2x2[5];

  Adj[8]  =   m13* Det2x2[6] - m14* Det2x2[7]  + m15* Det2x2[8];
  Adj[9]  = -(m12* Det2x2[6] - m14* Det2x2[9]  + m15* Det2x2[10]);
  Adj[10] =   m12* Det2x2[7] - m13* Det2x2[9]  + m15* Det2x2[11];
  Adj[11] = -(m12* Det2x2[8] - m13* Det2x2[10] + m14* Det2x2[11]);
  Adj[12] = -(m9 * Det2x2[6] - m10* Det2x2[7]  + m11* Det2x2[8]);
  Adj[13] =   m8 * Det2x2[6] - m10* Det2x2[9]  + m11* Det2x2[10];
  Adj[14] = -(m8 * Det2x2[7] - m9 * Det2x2[9]  + m11* Det2x2[11]);
  Adj[15] =   m8 * Det2x2[8] - m9 * Det2x2[10] + m10* Det2x2[11];

  // Finding the transposed adjoint matrix, which is the inverse

  float IDet = (1.0f / Det4x4);

  m_v[0] = IDet * Adj[0];  m_v[4] = IDet * Adj[1];  m_v[8]  = IDet * Adj[2];  m_v[12] = IDet * Adj[3];
  m_v[1] = IDet * Adj[4];  m_v[5] = IDet * Adj[5];  m_v[9]  = IDet * Adj[6];  m_v[13] = IDet * Adj[7];
  m_v[2] = IDet * Adj[8];  m_v[6] = IDet * Adj[9];  m_v[10] = IDet * Adj[10]; m_v[14] = IDet * Adj[11];
  m_v[3] = IDet * Adj[12]; m_v[7] = IDet * Adj[13]; m_v[11] = IDet * Adj[14]; m_v[15] = IDet * Adj[15];

}

static void swap(float * v, int pos1, int pos2)
{
  float tmp = v[pos1];
  v[pos1]=v[pos2];
  v[pos2]=tmp;
}

static void transpose (float * m)
{
  swap(m,1,4);
  swap(m,2,8);
  swap(m,3,12);
  swap(m,6,9);
  swap(m,7,13);
  swap(m,11,14);
}

void VMatrix::GetMatrixInverse (float* mit) const
{
  GetMatrix(mit);
  inverse(mit);
}

void VMatrix::GetMatrixIT (float* mit) const
{
  GetMatrixInverse(mit);
  transpose(mit);
}

static void get33LeftUp (float *src, float *dst)
{
  memcpy(dst, src, sizeof(float) * 3); // X column
	memcpy(dst + 3, src + 4, sizeof(float) * 3); // Y column
	memcpy(dst + 6, src + 8, sizeof(float) * 3); // Z column
}

void transpose3 (float *m)
{
  swap(m,1,3);
  swap(m,2,6);
  swap(m,5,7);
}

void VMatrix::GetNormalMatrix (float* dst)
{
  int cmode = mode; 
  mode = VM_MODELVIEW;
  float mit[16];
  GetMatrix(mit);
  mit[12] = 0;
  mit[13] = 0;
  mit[14] = 0;
  mit[3]  = 0;
  mit[7]  = 0;
  mit[11] = 0;
  inverse(mit);
  get33LeftUp (mit, dst);
  transpose3(dst);
  mode = cmode;
}

void VMatrix::SetActTex (bool act)
{
  tex = act;
}

void VMatrix::SetUniformVar (int p_id)
{
  int  mvp_id = glGetUniformLocation(p_id, "mvpMatrix");
  if(mvp_id == -1){
    printf("mvpMatrix not found.\n");
  }
  else{
    float mvp[16];
    multmatrix_aux(proj,top->m,mvp);
    glUniformMatrix4fv(mvp_id, 1, GL_FALSE, mvp);
  }
  int  mv_id = glGetUniformLocation(p_id, "mvMatrix");
  if(mv_id == -1){
    printf("mvMatrix not found.\n");
  }
  else
    glUniformMatrix4fv(mv_id, 1, GL_FALSE, top->m);
  int nm_id = glGetUniformLocation(p_id, "normalMatrix");
  if(nm_id==-1){
    printf("normalMatrix not found.\n");
  }
  else{
    float adj[9];
    GetNormalMatrix(adj);
    glUniformMatrix3fv(nm_id, 1, GL_FALSE, adj);
  }
  if(tex){
    int texLoc	= glGetUniformLocation(p_id, "tex0");
    glUniform1i(texLoc, 0);
  }
}

